package com.softwareengineering.trafficapplicationv2;


import android.graphics.Canvas;
import android.graphics.Color;

/**
 * This file is part of the Traffic Simulation Application.
 * The Traffic Simulation Application is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * The Traffic Simulation Application is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License
 * along with the Traffic Simulation Application.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * @file VehicleImage2D.java
 * @author (c) Martin Treiber, http://traffic-simulation.de/
 * @alterations David Black, Chris Nelson, and Garrett Stibbs, (c) December 03, 2012
 * @description Draw a 2D vehicle on the screen
 * @usage 	construct an image with VehicleImage2D(length_m, width_m, vehType, isACC, color, scale)
 * 			set state variables
 * 				setScale(scale) [optional]
 * 				setPosition(x_m, y_m)
 * 				setSpeed(speed_ms) [optional]
 * 				setDirection(phi) [optional]
 * @note	units of variables with name *_m in meters, otherwise pixels
 * 			physical units
 * 				x_m=0 <=> left
 * 				y_m=0 <=> bottom
 * 			pixel units
 * 				x=0 <=> left corner of drawing region
 * 				y=0 <=> top corner of drawing region
 * 
 */

public class VehicleImage2D implements Constants {

	static final boolean DRAWBLACKLINES = true;
	static final boolean DRAW_MOREBLACKLINES = true;
	static final double SPEEDMIN_MS=0;
	static final double SPEEDMAX_MS=30;
	static final int colorProbeveh= Color.rgb(40,0,255);
	
	// physical vehicle properties
	double length_m;
	double width_m;
	String vehType;	// {"car", "truck", "obstacle"}
	boolean isTruck;
	boolean isObstacle;
	boolean isACC;
	boolean isProbeveh;
	boolean isPerturbedveh;
	int vehColor;

	// optical vehicle properties
	double scale; // pixel units per meter
	int colorCode; // 0=physical color; 1=speed colormap; 2=ACC colored, others gray
	// state variables
	double posx_m, posy_m; // position of vehicle center in physical coordinate system
	int posx, posy; // pixel units
	double phi; // phi=0: in x direction (moving to the right) phi=pi/2: in y (upwards)

	double speed_ms;

	public static final int colormapSpeed(double v, double vmin, double vmax){
		// tune following values if not satisfied
		//(the floor function  of any hue value >=1 will be subtracted by HSBtoRGB)
		float hue_vmin=(float)(0.95);	// hue value for minimum speed value
		float hue_vmax=(float)(1.65);	// hue value for max speed (1 will be subtracted)

		float vRelative=(vmax>vmin) ? (float)( (v-vmin)/(vmax-vmin)) : 0;
		vRelative=Math.min(Math.max(0,vRelative), 1);
		float vRelativeNonlin=vRelative
				+(float)(0.1*(Math.cos(2*Math.PI*(vRelative+0.35))
						-Math.cos(2*Math.PI*(0.35)) ));
		float h=hue_vmin+vRelativeNonlin*(hue_vmax-hue_vmin);

		// use max saturation
		float s=(float)(1.0);

		// use max brightness
		float b=(float)(1.0);

		return GraphicsUtil.HSBtoRGB(h, s, b);
	}
	
	static int myround(double x) {
		return (int) Math.floor(x + 0.5);
	}

	public VehicleImage2D(double length_m, double width_m, String vehType, 
			boolean isACC, int vehColor, double scale){
		this.length_m = length_m;
		this.width_m = width_m;
		this.scale = scale;
		this.vehType = vehType;
		isTruck=(vehType.equals("truck"));
		isObstacle=(vehType.equals("obstacle"));
		this.isACC = isACC;
		this.vehColor = vehColor;
		speed_ms=0;
		phi=0;
		isProbeveh=false;
		isPerturbedveh=(vehColor == MicroStreet.colorPerturb);
	}

	public void draw(Canvas g, int colorCode) {
		this.colorCode=colorCode;

		// #########################################################
		// Design interface: all relative to center at 
		// (posx_m,posy_m) and phi=0 (driving in pos x direction)
		// #########################################################

		// projection of front and back windows and roof

		// vehType.equals("car")
		double xFrontWindow2_m = 0.25 * length_m; // 0.20 (var 0.20, LKW)
		double xFrontWindow1_m = 0.05 * length_m; // 0.05 (var 0.05, LKW)
		double xBackWindow1_m = -0.22 * length_m; // -0.22 (var-0.35, LKW)
		double xBackWindow2_m = -0.40 * length_m; // -0.35 (var-0.48, LKW-0.50)
		double yRoofLeft_m = 0.40 * width_m; // 0.40 (LKW 0.50)

		// vehType.equals("truck")
		if (vehType.equals("truck")) {
			xFrontWindow2_m = 0.30 * length_m; // 0.20 (var 0.20, LKW)
			xFrontWindow1_m = 0.25 * length_m; // 0.05 (var 0.05, LKW)
			xBackWindow1_m = -0.48 * length_m; // -0.22 (var-0.35, LKW)
			xBackWindow2_m = -0.49 * length_m; // -0.35 (var-0.48, LKW-0.50)
			yRoofLeft_m = 0.50 * width_m; // 0.40 (LKW 0.50)
		}

		// left "hood line"

		double xHood1_m = xFrontWindow2_m + 0.05 * length_m;
		double xHood2_m = 0.45 * length_m;
		double yHood1_m = 0.35 * width_m;
		double yHood2_m = 0.25 * width_m;

		// left mirror (front side of mirror attached at xFrontWindow2_m)

		double xMirr1_m = xFrontWindow2_m - 0.04 * length_m; // tip of mirror
		double xMirr2_m = xFrontWindow2_m - 0.03 * length_m; // "mirror" side
		double yMirr1_m = 0.60 * width_m;
		double yMirr2_m = 0.50 * width_m;

		// #########################################################
		//  end "design interface"
		// #########################################################

		// rel coord vehicle at posx=posy=phi=0 (driving in pos. x direction)

		// vehicle outline

		double halflen_m=0.5 * length_m;
		double halfw_m=0.5 * width_m;
		double xCorners_m[] = {-halflen_m, -halflen_m, halflen_m, halflen_m};
		double yCorners_m[] = { halfw_m, -halfw_m,-halfw_m, halfw_m};

		// front and back windows

		double xFrontw_m[] = {xFrontWindow1_m,xFrontWindow1_m,xFrontWindow2_m,xFrontWindow2_m };
		double yFrontw_m[] = {yRoofLeft_m, -yRoofLeft_m, -halfw_m, halfw_m };

		double xBackw_m[]  = {xBackWindow1_m, xBackWindow1_m, xBackWindow2_m, xBackWindow2_m };
		double yBackw_m[]  = {yRoofLeft_m, -yRoofLeft_m, -halfw_m, halfw_m};

		// roof

		double xRoof_m[] = { xBackWindow1_m, xBackWindow1_m, xFrontWindow1_m, xFrontWindow1_m };
		double yRoof_m[] = { yRoofLeft_m, -yRoofLeft_m, -yRoofLeft_m, yRoofLeft_m };

		// hood

		double xHood_m[] = { xHood1_m, xHood2_m, xHood1_m, xHood2_m };
		double yHood_m[] = { yHood1_m, yHood2_m, -yHood1_m, -yHood2_m };

		// mirrors

		double xMirrL_m[] = { xFrontWindow2_m, xMirr1_m, xMirr2_m };
		double yMirrL_m[] = { 0.50 * width_m, yMirr1_m, yMirr2_m };
		double xMirrR_m[] = { xFrontWindow2_m, xMirr1_m, xMirr2_m };
		double yMirrR_m[] = { -0.50 * width_m, -yMirr1_m, -yMirr2_m };

		// ####################################
		// draw: rotate + scale to pixels
		// ####################################

		double cp = Math.cos(-phi);  // negative angle since yPix direction downwards
		double sp = Math.sin(-phi);

		int xCorners[] = new int[4];
		int yCorners[] = new int[4];
		int xFrontw[] = new int[4];
		int yFrontw[] = new int[4];
		int xBackw[] = new int[4];
		int yBackw[] = new int[4];
		int xRoof[] = new int[4];
		int yRoof[] = new int[4];
		int xHood[] = new int[4];
		int yHood[] = new int[4];
		int xMirrL[] = new int[3];
		int yMirrL[] = new int[3];
		int xMirrR[] = new int[3];
		int yMirrR[] = new int[3];

		for (int i = 0; i < 4; i++) {
			xCorners[i] = posx + getdx(cp * xCorners_m[i] - sp * yCorners_m[i]);
			yCorners[i] = posy + getdy(sp * xCorners_m[i] + cp * yCorners_m[i]);

			xFrontw[i] = posx + getdx(cp * xFrontw_m[i] - sp * yFrontw_m[i]);
			yFrontw[i] = posy + getdy(sp * xFrontw_m[i] + cp * yFrontw_m[i]);

			xBackw[i] = posx + getdx(cp * xBackw_m[i] - sp * yBackw_m[i]);
			yBackw[i] = posy + getdy(sp * xBackw_m[i] + cp * yBackw_m[i]);

			xRoof[i] = posx + getdx(cp * xRoof_m[i] - sp * yRoof_m[i]);
			yRoof[i] = posy + getdy(sp * xRoof_m[i] + cp * yRoof_m[i]);
			xHood[i] = posx + getdx(cp * xHood_m[i] - sp * yHood_m[i]);
			yHood[i] = posy + getdy(sp * xHood_m[i] + cp * yHood_m[i]);
		}

		for (int i = 0; i < 3; i++) {
			xMirrL[i] = posx + getdx(cp * xMirrL_m[i] - sp * yMirrL_m[i]);
			yMirrL[i] = posy + getdy(sp * xMirrL_m[i] + cp * yMirrL_m[i]);
			xMirrR[i] = posx + getdx(cp * xMirrR_m[i] - sp * yMirrR_m[i]);
			yMirrR[i] = posy + getdy(sp * xMirrR_m[i] + cp * yMirrR_m[i]);
		}

		// #########################################
		// draw: actually paint the pixels
		// #########################################

		int drawColor=(isObstacle)? Color.BLACK
				: ((colorCode==0)||((colorCode==1)&&isTruck)) ? vehColor
						: (colorCode==1) ? colormapSpeed(speed_ms, SPEEDMIN_MS, SPEEDMAX_MS)
								: (isACC) ? vehColor : Color.rgb(127,127,127);  
		if(isPerturbedveh) drawColor=vehColor; //info "isPerturbed" by vehColor
		if(isProbeveh) drawColor=colorProbeveh;
		
		GraphicsUtil.fillPolygon(g, xCorners, yCorners, 4, drawColor);
		
		if(!isObstacle){
			GraphicsUtil.fillPolygon(g, xFrontw, yFrontw, 4, GraphicsUtil.brighter(GraphicsUtil.brighter(drawColor)));
			GraphicsUtil.fillPolygon(g, xBackw, yBackw, 4, GraphicsUtil.darker(GraphicsUtil.darker(drawColor)));

			int w = getdx(width_m);
			boolean draw_blacklines = (DRAWBLACKLINES && (w > 3)); // outline
			boolean draw_more_blacklines = (DRAW_MOREBLACKLINES && (w > 8)); // hood etc
			boolean draw_mirrors = false;

			if (draw_blacklines) {
				GraphicsUtil.drawPolygon(g,  xCorners, yCorners, 4, Color.BLACK);
				if (draw_more_blacklines) {
					GraphicsUtil.drawPolygon(g,  xFrontw, yFrontw, 4, Color.BLACK);
					GraphicsUtil.drawPolygon(g,  xBackw, yBackw, 4, Color.BLACK);
					GraphicsUtil.drawPolygon(g,  xRoof, yRoof, 4, Color.BLACK);
					GraphicsUtil.drawLine(g, xHood[0], yHood[0], xHood[1], yHood[1], Color.BLACK);
					GraphicsUtil.drawLine(g, xHood[2], yHood[2], xHood[3], yHood[3], Color.BLACK);
					if (draw_mirrors) {
						GraphicsUtil.drawPolygon(g,  xMirrL, yMirrL, 3, Color.BLACK);
						GraphicsUtil.drawPolygon(g,  xMirrR, yMirrR, 3, Color.BLACK);
					}
				}
			}
		}
	}

	private int getdx(double dx_m) {
		return (myround(scale * dx_m));
	}

	private int getdy(double dy_m) {
		return (myround(-scale * dy_m));
	}

	private int getx(double x_m) {
		return (myround(scale * x_m));
	}

	private int gety(double y_m) {
		return (myround(-scale * y_m));
	}

	public void setDirection(double phi) {
		this.phi = phi;
	}

	public void setPerturbedveh() {
		this.isPerturbedveh=true;
	}

	public void setPosition(double posx_m, double posy_m) {
		this.posx_m = posx_m;
		this.posy_m = posy_m;
		posx = getx(posx_m);
		posy = gety(posy_m);

	}

	public void setPositionPix(int posx_pix, int posy_pix) {
		this.posx = posx_pix;
		this.posy = posy_pix;
	}

	public void setProbeveh() {
		this.isProbeveh=true;
	}

	public void setScale(double scale) {
		this.scale = scale;
	}

	public void setSpeed(double speed){
		this.speed_ms=speed;
	}

}
